#include <iostream>
#include <stdio.h>
#include <cmath>
#include <string>
#include <vector>

#include "ros/ros.h"
#include <rosbag/bag.h>
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sensor_msgs/Image.h>
#include <time.h>

#define BASE_TIME 1609307878

using namespace std;
clock_t start,end;


int main(int argc, char** argv)
{
    ros::init(argc, argv, "image_node");
    ros::NodeHandle n("~");

    rosbag::Bag bag;
    bag.open("kalibr_image.bag" , 1U) ;
    printf("read dataPath: %s\n", argv[1]);
    string dataPath = argv[1];

    FILE* file;
    cout << dataPath+"/frame_timestamps_image.txt" << endl;
    file = std::fopen((dataPath+"/frame_timestamps_image.txt").c_str(), "r");
    if(file == NULL)
    {
	    printf("cannot find file: %s \n", dataPath.c_str());
	    ROS_BREAK();
	    return 0;          
	}

    vector<long> imageTimeList;
    //char s[100];
    // int d = fscanf(file, "%[^\n]%*c", s);
    // printf("%s", s);
    long nanosec;
    char str1[100], str2[100];
    // char string_Temp[60];
    // for(int i=0;i<3;i++)
    // {
    //     fscanf(file, "%s",&string_Temp);
    // }
    while(fscanf(file, "%ld ", &nanosec ) != EOF)
    {
        //double sec = nanosec/1000000000.0;
        imageTimeList.push_back(nanosec);
        cout << nanosec << endl;
    }
    cout << imageTimeList.size() << endl;
    std::fclose(file);

    string leftImagePath, rightImagePath;
    cv::Mat imleft_src, imright_src;
    cv::Mat imLeft, imRight;
    clock_t start,end;
    for(int i = 0; i < imageTimeList.size(); i++)
    {
        start = clock();  
        if(ros::ok())
        {
            printf("\nprocess image %d\n", i);
            stringstream ss;
            ss << imageTimeList[i];
            //cout << ss.str() <<endl;
            //交换左右目
            leftImagePath = dataPath + "/leftImage/" + ss.str() + ".png";
            rightImagePath = dataPath + "/rightImage/" + ss.str() + ".png";

            double time = BASE_TIME + imageTimeList[i]/1000000000.0;

            imleft_src = cv::imread(leftImagePath, cv::IMREAD_GRAYSCALE );
            //翻转图像
            //imLeft = imleft_src.clone();
            cv::flip(imleft_src, imLeft, -1);
            //ROS_INFO("flip!");
            sensor_msgs::ImagePtr imLeftMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imLeft).toImageMsg();
            imLeftMsg->header.stamp = ros::Time(time);
            bag.write("left", imLeftMsg->header.stamp, imLeftMsg);

            imright_src = cv::imread(rightImagePath, cv::IMREAD_GRAYSCALE );
            //imRight = imright_src.clone();
            //翻转图像
            cv::flip(imright_src, imRight, -1);
			sensor_msgs::ImagePtr imRightMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imRight).toImageMsg();
			imRightMsg->header.stamp = ros::Time(time);
			bag.write("right", imRightMsg->header.stamp, imRightMsg);
        }
        else
            break;
        end = clock();
        double during =  double(end-start)/CLOCKS_PER_SEC;
        double SleepTime = 0.1 - during;
        if(SleepTime > 0)
            ros::Duration(SleepTime).sleep();
    }
    return 0;
}
